#include "plant_1DSPDE_input_1Dsin.h"

// --------------------------------------------------------------
// plant1D_SPDE_input_1Dsin::plant1D_SPDE_input_1Dsin(int M, int L,
// 						   double idt,double iXmax):
plant1D_SPDE_input_1Dsin::plant1D_SPDE_input_1Dsin(int M, int L,
						   double idt,int aggre):
f_c2(L), f_sigma2(L)
{
  int i,m;
  // MODEL PARAMTERS --------------------------
  // Xmax = iXmax;
  Xmax = double(L);

  // NUMERICAL SOLUTION PARAMETERS ------------
  mode = M;	
  LatticeSize = aggre;

  // BASIC BLOCK ------------------------------
  // Input#0 = T (K)
  // Input#1 = W (layer/s)
  // Input#2 = A (layer/s)
  // Input#3 = Freq
  int idim[4] = {1,1,1,1};
  // Output#0 = h (layer/s)
  // Output#1 = r2 (layer^2)
  // Output#2 = m2
  // Output#3 = a (site)
  // Output#4~#5 = values of eigenfunction phi_1 ~ phi_2
  int odim[6];
  odim[0] = 1;
  odim[1] = 1;
  odim[2] = 1;
  odim[3] = 1;
  odim[4] = (mode+1); // The mode with sin*sin
  odim[5] = (mode+1);
  block::initialize(4,idim,6,odim,idt);

  // STATES -----------------------------------
  z1 = block::outputs[4];
  z2 = block::outputs[5];
  z1_next = new double[(mode+1)];
  z2_next = new double[(mode+1)];
  for(i=0;i<(mode+1);i++){
    z1_next[i] = 0.0;
    z2_next[i] = 0.0;
  }

  // MODEL PARAMETERS -----------------------------------
  lambda = new double [mode+1];
  for(m=0;m<mode+1;m++){
    // lambda[m] = new double;
  }

  for(i=0;i<2;i++){
    Km2[i] = new double[mode+1];
  }

  double dphi;
  int ix,iy,ix_next;
  int index;
  double temp;
  for(m=0;m<=mode;m++){
    temp = 4*sin(pi*double(m)/double(LatticeSize))*sin(pi*double(m)/double(LatticeSize))/(Xmax*Xmax*Xmax/LatticeSize/LatticeSize);
    index = m;
    //Version 1
    //for(i=0;i<4;i++){
    //  Km2[i][index] = 0.0;
    //}
    //for(ix=0;ix<LatticeSize;ix++){
    //  for(iy=0;iy<LatticeSize;iy++){
    //    if(ix == LatticeSize-1){ix_next = 0;}
    //    else{ix_next = ix+1;}
    //    dphi = phi1mn(m,n,ix_next,iy)-phi1mn(m,n,ix,iy);
    //    Km2[0][index] += dphi*dphi;
    //    dphi = phi2mn(m,n,ix_next,iy)-phi2mn(m,n,ix,iy);
    //    Km2[1][index] += dphi*dphi;
    //    dphi = phi3mn(m,n,ix_next,iy)-phi3mn(m,n,ix,iy);
    //    Km2[2][index] += dphi*dphi;
    //    dphi = phi4mn(m,n,ix_next,iy)-phi4mn(m,n,ix,iy);
    //    Km2[3][index] += dphi*dphi;
    //  }
    //}
    //Km2[0][index] = Km2[0][index]/(XYmax*XYmax); //FIXME
    //Km2[1][index] = Km2[1][index]/(XYmax*XYmax);
    //Km2[2][index] = Km2[2][index]/(XYmax*XYmax);
    //Km2[3][index] = Km2[3][index]/(XYmax*XYmax);

    // Version 2
    //Km2[0][index] = 4*sin(pi*m/LatticeSize)*sin(pi*m/LatticeSize)/(XYmax*XYmax);
    //Km2[1][index] = Km2[0][index];
    //Km2[2][index] = Km2[0][index];
    //Km2[3][index] = Km2[0][index];

    // Version 3

    for(i=0;i<2;i++)
    {
      switch(i)
      {
      case 0:
	if(m==0)
	{
	  Km2[i][index] = 0.0;
	}else
	{
	  Km2[i][index] = temp;
	}
	break;
      case 1:
	if(m==0||2*m==LatticeSize)
	{
	  Km2[i][index] = 0.0;
	}else
	{
	  Km2[i][index] = temp;
	}
      }
    }

  }

  // SIMULATIN PARAMETERS -------------------------
  // For analytical expression
  sqrt_dt = sqrt(block::dt);
  // For explicit/implicit method
  dt_con = 0.0001;
  sqrt_dt_con = sqrt(dt_con);
  dt_conservative_factor = 0.1;

  gsl_rng_env_setup();
  noise_type = gsl_rng_default;
  noise = gsl_rng_alloc(noise_type);
  gsl_rng_set(noise,time_seed());

  SQRT_ONE_OVER_LX = sqrt(1/Xmax);
  SQRT_TWO_OVER_LX = sqrt(2/Xmax);

  // INITIALIZATION -------------------------------
  reset();
}
// --------------------------------------------------------------
plant1D_SPDE_input_1Dsin::~plant1D_SPDE_input_1Dsin(){
  int m;
  for(int i=0;i<2;i++){
    delete []Km2[i];
  }

  for(m=0;m<mode+1;m++){
    //    delete lambda[m];
  }

  delete []z1_next;
  delete []z2_next;

  delete []lambda;
  gsl_rng_free(noise);
}
// --------------------------------------------------------------
void plant1D_SPDE_input_1Dsin::reset(){
  block::time = 0.0;
  int m;
  // Reset states
  for(m=0;m<mode+1;m++){
    z1[index1D(m)] = 0.0;
    z2[index1D(m)] = 0.0;
  }
  // Reset outputs
  outputs[0][0] = getH();
  outputs[1][0] = getR2();
  outputs[2][0] = getM2();
  outputs[3][0] = getCorLength();
}
// --------------------------------------------------------------
void plant1D_SPDE_input_1Dsin::update(double sysTime){
  if(sysTime<time){
    cerr << "Error@plant1D_SPDE_input_1Dsin::update(), sysTime = " << sysTime << ", time = " << time << "\n";
    exit(1);
  }

  // Update states  ====================================
  int i,m;
  int index;
  double c2;
  double sigma2;
  double iT,iWmean;

  while(time+dt<=sysTime || time == 0){
    if(sysTime!=0){
      // x_{k} <== x_{k+1}
      for(i=0;i<(mode+1);i++){
	z1[i] = z1_next[i];
	z2[i] = z2_next[i];
      } 
    }

    // Calculate x_{k+1} ---------------------------
    iWmean = inputs[0][0];
    A = inputs[1][0];
    Freq = inputs[2][0];
    iT = inputs[3][0];
    // UPDATE MODEL PARAMETERS ---------------------
    if(iT!=T ||iWmean!=W){
      T = iT;
      W = iWmean;
      c2 = f_c2.value(&W);
      // sigma2 = sqrt(eq_sigma2(T,W)); // A BIG BUG
      sigma2 = f_sigma2.value(&W);
      for(m=0;m<mode+1;m++)
      {
	lambda[m]= -4*c2*(m*m)*pi*pi/(Xmax*Xmax);
	//	  dt_con[m][n] = -2/lambda[m][n]*dt_conservative_factor;  

      }
    }
    else{
      c2 = f_c2.value(&W);
      sigma2 = f_sigma2.value(&W);
    }

    // UPDATE STATES -------------------------------
    for(m=0;m<=mode;m++){
      index = index1D(m);

      // Implicit method =====================================
      /*	if(m>0&&n>0){
      // z1[index] = z1[index]+lambda[m][n]*z1[index]*dt_con+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con;
      z1[index] = (z1[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con)/(1-lambda[m][n]*dt_con);
      }

      if(m==0&&n==0){
      //	  z2[index] = z2[index]+c_200*dt_con+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con;
      double w200 = iWmean*XYmax*(1-(cos(2*iFreq*pi)-1)/(2*iFreq*pi));
      z2[index] = (z2[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con+w200)/(1-lambda[m][n]*dt_con);
      }
      else if(n!=0 || iFreq==m){
      //	  z2[index] = z2[index]+lambda[m][n]*z2[index]*dt_con+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con;
      z2[index] = (z2[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con)/(1-lambda[m][n]*dt_con);
      }
      else{// n = 0 && iFreq!=m
      double w2m0 = SQRT_TWO*iWmean*XYmax*iFreq*cos(2*iFreq*pi)/(2*pi*(m*m-iFreq*iFreq));
      z2[index] = (z2[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con+w2m0)/(1-lambda[m][n]*dt_con);
      }

      if(m>0){
      if(n!=0){
      //	    z3[index] = z3[index]+lambda[m][n]*z3[index]*dt_con+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con;
      z3[index] = (z3[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con)/(1-lambda[m][n]*dt_con);
      }
      else{
      if(m==iFreq){
      double w3k0 = SQRT_TWO*iWmean*XYmax/2;
      z3[index] = (z3[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con+w3k0)/(1-lambda[m][n]*dt_con);
      }
      else{//m!=iFreq
      double w3m0 = SQRT_TWO*iWmean*XYmax*m*sin(2*iFreq*pi)/(2*pi*(iFreq*iFreq-m*m));
      z3[index] = (z3[index]+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con+w3m0)/(1-lambda[m][n]*dt_con);
      }
      }

      If(n>0){
      //	  z4[index] = z4[index]+lambda[m][n]*z4[index2D(m,n)]*dt_con+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con;
      z4[index] = (z4[index]+gsl-ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt_con)/(1-lambda[m][n]*dt_con);
      }
      }*/
      // Analytical expression ==============================
      double temp1 = exp(lambda[m]*dt);
      double temp2 = sqrt(sigma2*(exp(2.0*lambda[m]*dt)-1.0)/(2.0*lambda[m]));
      double temp3 = (temp1-1.0)/lambda[m];

      /*if(m>0)
      {
      z1_next[index] = z1[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);
      }

      if(m==0)
      {
      double w10 = W*Xmax+A*Xmax*(1-cos(2*Freq*pi))/(2*Freq*pi);
      z2_next[index] = z2[index]+
      w10*dt+gsl_ran_gaussian_ziggurat(noise,1.0)*sigma2*sqrt_dt;
      }
      else
      {
      if(Freq!=m)
      {
      double w1m = SQRT_TWO*A*Xmax*Freq*(cos(2*Freq*pi)-1)/(2*pi*(m*m-Freq*Freq));
      z2_next[index] = z2[index]*temp1+
      temp2*gsl_ran_gaussian_ziggurat(noise,1.0)+temp3*w1m;
      }
      else
      {
      z2_next[index] = z2[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);
      }
      }*/

      if(m==Freq)
      {
	double w1k0 = A*sqrt(Xmax/2);
	z1_next[index] = z1[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0)+temp3*w1k0;
      }else if(m==0)
      {
	z1_next[index] = 0;			
      }else
      {
	z1_next[index] = z1[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);
      }

      if(m==0)
      {
	double w20 = W*sqrt(1/Xmax)+A*sqrt(1/Xmax)*Xmax*(1-cos(2*Freq*pi))/(2*Freq*pi);
	z2_next[index] = z2[index]+w20*dt+gsl_ran_gaussian_ziggurat(noise,1.0)*sqrt(sigma2)*sqrt_dt;
      }
      else
      {
	z2_next[index] = z2[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);  
      }



      /*if(m>0)
      {
      if(n!=0)
      {
      z3_next[index] = z3[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);
      }
      else
      {
      if(m==Freq)
      {
      double w3k0 = SQRT_TWO*A*XYmax/2;
      z3_next[index] = z3[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0)+temp3*w3k0;
      }
      else
      {
      double w3m0 = SQRT_TWO*A*XYmax*m*sin(2*Freq*pi)/(2*pi*(Freq*Freq-m*m));
      z3_next[index] = z3[index]*temp1+
      temp2*gsl_ran_gaussian_ziggurat(noise,1.0)+temp3*w3m0;
      }
      }
      }
      else
      {
      z3_next[index] = 0.0;
      }

      if(n>0)
      {
      z4_next[index] = z4[index]*temp1+temp2*gsl_ran_gaussian_ziggurat(noise,1.0);
      }
      else
      {
      z4_next[index] = 0.0;
      }*/

    }// end of for(m = ...)
    // ==========================================
    if(time==0)
      break;
    else{
      time=time+dt;
    }
    // ==========================================
  }// end of while

  // OUTPUTS -----------------------------------
  outputs[0][0] = getH();
  outputs[1][0] = getR2();
  outputs[2][0] = getM2();
  outputs[3][0] = getCorLength();
  return;
}
// --------------------------------------------------------------
void plant1D_SPDE_input_1Dsin::output(string FileName){
  ofstream fout;
  fout.open(FileName.c_str());
  fout << "Mode = " << mode;
  fout << "Time = " << block::time;
  int i;

  int dim = (mode+1);

  for(i=0;i<dim;i++){
    fout << z1[i] << " ";
  }
  fout << "\n";

  for(i=0;i<dim;i++){
    fout << z2[i] << " ";
  }
  fout << "\n";


}
// --------------------------------------------------------------
double plant1D_SPDE_input_1Dsin::getM2(){
  int m,index;
  double m2 = 0.0;
  for(m=1;m<=mode;m++){  
    index = index1D(m);
    m2 +=  Km2[0][index]*z1[index]*z1[index]
    +Km2[1][index]*z2[index]*z2[index];
  }
  return m2;
}
// --------------------------------------------------------------
double plant1D_SPDE_input_1Dsin::getR2(){
  int m,n,index;
  double r2 = 0.0;
  for(m=1;m<=mode;m++){
    index = index1D(m);
    r2 +=  z1[index]*z1[index]
    +z2[index]*z2[index];
  }
  r2 /= (Xmax);
  return r2;
}
// --------------------------------------------------------------
double plant1D_SPDE_input_1Dsin::getH(){
  double hmean;
  hmean = z2[index1D(0)]/Xmax;
  return hmean;
}
// --------------------------------------------------------------
double plant1D_SPDE_input_1Dsin::getCorLength(){
  double r2 = outputs[1][0];
  double m2 = outputs[2][0];
  return sqrt(2*r2/m2);
}
// --------------------------------------------------------------
void plant1D_SPDE_input_1Dsin::getSnapshot(string FileName){
  ofstream fsnapshot(FileName.c_str());
  int ix,im;
  double h;
  int index;
  for(ix=0;ix<LatticeSize;ix++)
  {
    h = 0;
    for(im=0;im<=mode;im++)
    {
      index = index1D(im);
      h += 
	phi1m(im,ix)*z1[index]+
	phi2m(im,ix)*z2[index];
    }
    fsnapshot << setw(10) << h;
    if(ix!=LatticeSize-1)
    {
      fsnapshot << "\n";
    }
  }
  fsnapshot.close();
}
// --------------------------------------------------------------
//double plant1D_SPDE_input_1Dsin::eq_c2(double W){
//	double a_c2=-60.8403;
//	double b_c2=-62.1356;
//	double c_c2=79.1405;
//	double d_c2=-25.2973;
//	double e_c2=9.0313;
//	
//  return exp(a_c2*pow(W,4)+b_c2*pow(W,3)+c_c2*pow(W,2)+d_c2*pow(W,1)+e_c2);  //parameters fitting to rough2
//
//	/*double a_c2=23.9948;
//	double b_c2=26.4299;
//	double c_c2=-32.58;
//	double d_c2=8.2319;
//	double e_c2=-0.112;
//
//	return a_c2*pow(W,4)+b_c2*pow(W,3)+c_c2*pow(W,2)+d_c2*pow(W,1)+e_c2;*/ //parameters fitting to slope2
//}
//
//double plant1D_SPDE_input_1Dsin::eq_sigma2(double W){
//
//	double a_sigma2=2.4851;
//	double b_sigma2=0.3627;
//    return a_sigma2*W+b_sigma2; //parameters fitting to rough2
//
//	
//	/*double a_sigma2=97.0163;
//	double b_sigma2=-66.2908;
//	double c_sigma2=8.0974;
//	double d_sigma2=1.916;
//	double e_sigma2=-0.0296;
//
//	return a_sigma2*pow(W,4)+b_sigma2*pow(W,3)+c_sigma2*pow(W,2)+d_sigma2*pow(W,1)+e_sigma2;*/  //parameters fitting to slope2
//}
// -------------------------------------------------------------
